
#include "linux/slab.h"
#include "linux/printk.h"
#include <linux/irqdesc.h>
#include <linux/of.h>
#include <linux/errno.h>
#include <linux/irq.h>
#include <linux/irqdomain.h>

static LIST_HEAD(irq_domain_list);
// static DEFINE_MUTEX(irq_domain_mutex);
// static DEFINE_MUTEX(revmap_trees_mutex);
static struct irq_domain *irq_default_domain;

struct irq_data *irq_domain_get_irq_data(struct irq_domain *domain, unsigned int virq)
{
    struct irq_data *irq_data = irq_get_irq_data(virq);

    return (irq_data && irq_data->domain == domain) ? irq_data : NULL;
}

unsigned int irq_find_mapping(struct irq_domain *domain, irq_hw_number_t hwirq)
{
    struct irq_data *data;

    /* Look for default domain if nececssary */
    if (domain == NULL)
        domain = irq_default_domain;
    if (domain == NULL)
        return 0;

    if (hwirq < domain->revmap_direct_max_irq)
    {
        data = irq_domain_get_irq_data(domain, hwirq);
        if (data && data->hwirq == hwirq)
            return hwirq;
    }

    /* Check if the hwirq is in the linear revmap. */
    if (hwirq < domain->revmap_size)
        return domain->linear_revmap[hwirq];

    // rcu_read_lock();
    data = radix_tree_lookup(&domain->revmap_tree, hwirq);
    // rcu_read_unlock();
    return data ? data->irq : 0;
}

struct irq_domain *irq_find_host(struct device_node *node)
{
    struct irq_domain *h, *found = NULL;
    int rc;

    //mutex_lock(&irq_domain_mutex);
    list_for_each_entry(h, &irq_domain_list, link)
    {
        if (h->ops->match)
            rc = h->ops->match(h, node);
        else
            rc = (h->of_node != NULL) && (h->of_node == node);

        if (rc)
        {
            found = h;
            break;
        }
    }
    //mutex_unlock(&irq_domain_mutex);
    return found;
}

static int irq_domain_is_hierarchy(struct irq_domain *domain)
{
    return domain->flags & IRQ_DOMAIN_FLAG_HIERARCHY;
}

static struct irq_data *irq_domain_insert_irq_data(struct irq_domain *domain, struct irq_data *child)
{
	struct irq_data *irq_data;

	irq_data = kzalloc(sizeof (struct irq_data), GFP_KERNEL);
    if (irq_data)
    {
        child->parent_data = irq_data;
        irq_data->irq = child->irq;
        irq_data->node = child->node;
        irq_data->domain = domain;
    }

    return irq_data;
}

static void irq_domain_free_irq_data(unsigned int virq, unsigned int nr_irqs)
{
    struct irq_data *irq_data, *tmp;
    int i;

    for (i = 0; i < nr_irqs; i++)
    {
        irq_data = irq_get_irq_data(virq + i);
        tmp = irq_data->parent_data;
        irq_data->parent_data = NULL;
        irq_data->domain = NULL;

        while (tmp)
        {
            irq_data = tmp;
            tmp = tmp->parent_data;
            kfree(irq_data);
        }
    }
}

static int irq_domain_alloc_irq_data(struct irq_domain *domain,  unsigned int virq, unsigned int nr_irqs)
{
    struct irq_data *irq_data;
    struct irq_domain *parent;
    int i;

    /* The outermost irq_data is embedded in struct irq_desc */
    for (i = 0; i < nr_irqs; i++)
    {
        irq_data = irq_get_irq_data(virq + i);
        irq_data->domain = domain;

        for (parent = domain->parent; parent; parent = parent->parent)
        {
            irq_data = irq_domain_insert_irq_data(parent, irq_data);
            if (!irq_data)
            {
                irq_domain_free_irq_data(virq, i + 1);
                return -ENOMEM;
            }
        }
    }

    return 0;
}

static int irq_domain_alloc_descs(int virq, unsigned int cnt, irq_hw_number_t hwirq, int node)
{
    unsigned int hint;

    if (virq >= 0)
    {
        virq = irq_alloc_descs(virq, virq, cnt, node);
    }
    else
    {
        hint = hwirq % nr_irqs;
        if (hint == 0)
            hint++;
        virq = irq_alloc_descs_from(hint, cnt, node);
        if (virq <= 0 && hint > 1)
            virq = irq_alloc_descs_from(1, cnt, node);
    }

    return virq;
}

static int irq_domain_is_auto_recursive(struct irq_domain *domain)
{
	return domain->flags & IRQ_DOMAIN_FLAG_AUTO_RECURSIVE;
}

static void irq_domain_free_irqs_recursive(struct irq_domain *domain,
                                           unsigned int irq_base,
                                           unsigned int nr_irqs)
{
    domain->ops->free(domain, irq_base, nr_irqs);
    if (irq_domain_is_auto_recursive(domain))
    {
        //BUG_ON(!domain->parent);
        irq_domain_free_irqs_recursive(domain->parent, irq_base, nr_irqs);
    }
}

static int irq_domain_alloc_irqs_recursive(struct irq_domain *domain,
                                           unsigned int irq_base,
                                           unsigned int nr_irqs, void *arg)
{
    int ret = 0;
    struct irq_domain *parent = domain->parent;
    int recursive = irq_domain_is_auto_recursive(domain);

    //BUG_ON(recursive && !parent);
    if (recursive)
        ret = irq_domain_alloc_irqs_recursive(parent, irq_base,
                                              nr_irqs, arg);
    if (ret >= 0)
        ret = domain->ops->alloc(domain, irq_base, nr_irqs, arg);
    if (ret < 0 && recursive)
        irq_domain_free_irqs_recursive(parent, irq_base, nr_irqs);

    return ret;
}


static void irq_domain_insert_irq(int virq)
{
	struct irq_data *data;

    for (data = irq_get_irq_data(virq); data; data = data->parent_data)
    {
        struct irq_domain *domain = data->domain;
        irq_hw_number_t hwirq = data->hwirq;

        if (hwirq < domain->revmap_size)
        {
            domain->linear_revmap[hwirq] = virq;
        }
        else
        {
            //mutex_lock(&revmap_trees_mutex);
            //radix_tree_insert(&domain->revmap_tree, hwirq, data);
            //mutex_unlock(&revmap_trees_mutex);
        }

        /* If not already assigned, give the domain the chip's name */
        if (!domain->name && data->chip)
            domain->name = data->chip->name;
    }

    //irq_clear_status_flags(virq, IRQ_NOREQUEST);
}


int __irq_domain_alloc_irqs(struct irq_domain *domain, int irq_base,
                            unsigned int nr_irqs, int node, void *arg,
                            int realloc)
{
    int i, ret, virq;

    if (domain == NULL)
    {
        domain = irq_default_domain;
        if (WARN(!domain, "domain is NULL; cannot allocate IRQ\n"))
            return -EINVAL;
    }

    if (!domain->ops->alloc)
    {
        printk("domain->ops->alloc() is NULL\n");
        return -ENOSYS;
    }

    if (realloc && irq_base >= 0)
    {
        virq = irq_base;
    }
    else
    {
        virq = irq_domain_alloc_descs(irq_base, nr_irqs, 0, node);
        if (virq < 0)
        {
            printk("cannot allocate IRQ(base %d, count %d)\n",
                     irq_base, nr_irqs);
            return virq;
        }
    }

    if (irq_domain_alloc_irq_data(domain, virq, nr_irqs))
    {
        printk("cannot allocate memory for IRQ%d\n", virq);
        ret = -ENOMEM;
        goto out_free_desc;
    }

    // mutex_lock(&irq_domain_mutex);
    ret = irq_domain_alloc_irqs_recursive(domain, virq, nr_irqs, arg);
    if (ret < 0)
    {
        // mutex_unlock(&irq_domain_mutex);
        goto out_free_irq_data;
    }
    for (i = 0; i < nr_irqs; i++)
        irq_domain_insert_irq(virq + i);
    // mutex_unlock(&irq_domain_mutex);

    return virq;

out_free_irq_data:
    irq_domain_free_irq_data(virq, nr_irqs);
out_free_desc:
    irq_free_descs(virq, nr_irqs);
    return ret;
}

int irq_domain_alloc_irqs(struct irq_domain *domain, unsigned int nr_irqs, int node, void *arg)
{
    return __irq_domain_alloc_irqs(domain, -1, nr_irqs, node, arg, false);
}

int irq_domain_associate(struct irq_domain *domain, unsigned int virq,  irq_hw_number_t hwirq)
{
    struct irq_data *irq_data = irq_get_irq_data(virq);
    int ret;

    if (WARN(hwirq >= domain->hwirq_max,"error: hwirq 0x%x is too large for %s\n", (int)hwirq, domain->name))
        return -EINVAL;
    if (WARN(!irq_data, "error: virq%i is not allocated", virq))
        return -EINVAL;
    if (WARN(irq_data->domain, "error: virq%i is already associated", virq))
        return -EINVAL;

    //mutex_lock(&irq_domain_mutex);
    irq_data->hwirq = hwirq;
    irq_data->domain = domain;
    if (domain->ops->map)
    {
        ret = domain->ops->map(domain, virq, hwirq);
        if (ret != 0)
        {
            /*
             * If map() returns -EPERM, this interrupt is protected
             * by the firmware or some other service and shall not
             * be mapped. Don't bother telling the user about it.
             */
            if (ret != -EPERM)
            {
                printk("%s didn't like hwirq-0x%lx to VIRQ%i mapping (rc=%d)\n",
                        domain->name, hwirq, virq, ret);
            }
            irq_data->domain = NULL;
            irq_data->hwirq = 0;
            //mutex_unlock(&irq_domain_mutex);
            return ret;
        }

        /* If not already assigned, give the domain the chip's name */
        if (!domain->name && irq_data->chip)
            domain->name = irq_data->chip->name;
    }

    if (hwirq < domain->revmap_size)
    {
        domain->linear_revmap[hwirq] = virq;
    }
    else
    {
        //mutex_lock(&revmap_trees_mutex);
        //radix_tree_insert(&domain->revmap_tree, hwirq, irq_data);
        //mutex_unlock(&revmap_trees_mutex);
    }
    //mutex_unlock(&irq_domain_mutex);

    //irq_clear_status_flags(virq, IRQ_NOREQUEST);

    return 0;
}

void irq_domain_associate_many(struct irq_domain *domain, unsigned int irq_base, irq_hw_number_t hwirq_base, int count)
{
	int i;

	printk("%s(%s, irqbase=%i, hwbase=%i, count=%i)\n", __func__,
		of_node_full_name(domain->of_node), irq_base, (int)hwirq_base, count);


	for (i = 0; i < count; i++) {
		irq_domain_associate(domain, irq_base + i, hwirq_base + i);
	}
}

unsigned int irq_create_mapping(struct irq_domain *domain, irq_hw_number_t hwirq)
{
    int virq;

    printk("irq_create_mapping(0x%p, 0x%lx)\n", domain, hwirq);

    /* Look for default domain if nececssary */
    if (domain == NULL)
        domain = irq_default_domain;
    if (domain == NULL)
    {
        WARN(1, "%s(, %lx) called with NULL domain\n", __func__, hwirq);
        return 0;
    }
    printk("-> using domain @%p\n", domain);

    /* Check if mapping already exists */
    virq = irq_find_mapping(domain, hwirq);
    if (virq)
    {
        printk("-> existing mapping on virq %d\n", virq);
        return virq;
    }

    /* Allocate a virtual interrupt number */
    virq = irq_domain_alloc_descs(-1, 1, hwirq, -1);
    if (virq <= 0)
    {
        printk("-> virq allocation failed\n");
        return 0;
    }

    if (irq_domain_associate(domain, virq, hwirq))
    {
        irq_free_desc(virq);
        return 0;
    }

    printk("irq %ld on domain %s mapped to virtual irq %d\n",
             hwirq, of_node_full_name(domain->of_node), virq);

    return virq;
}

unsigned int irq_create_of_mapping(struct of_phandle_args *irq_data)
{
    struct irq_domain *domain;
    irq_hw_number_t hwirq;
    unsigned int type = IRQ_TYPE_NONE;
    int virq;

    domain = irq_data->np ? irq_find_host(irq_data->np) : irq_default_domain;
    if (!domain)
    {
        printk("no irq domain found for %s !\n", of_node_full_name(irq_data->np));
        return 0;
    }

    /* If domain has no translation, then we assume interrupt line */
    if (domain->ops->xlate == NULL)
        hwirq = irq_data->args[0];
    else
    {
        if (domain->ops->xlate(domain, irq_data->np, irq_data->args, irq_data->args_count, &hwirq, &type))
            return 0;
    }

    if (irq_domain_is_hierarchy(domain))
    {
        /*
         * If we've already configured this interrupt,
         * don't do it again, or hell will break loose.
         */
        virq = irq_find_mapping(domain, hwirq);
        if (virq)
            return virq;

        virq = irq_domain_alloc_irqs(domain, 1, -1, irq_data);
        if (virq <= 0)
            return 0;
    }
    else
    {
        /* Create mapping */
        virq = irq_create_mapping(domain, hwirq);
        if (!virq)
            return virq;
    }

    /* Set type if specified and different than the current one */
    if (type != IRQ_TYPE_NONE && type != irq_get_trigger_type(virq))
        irq_set_irq_type(virq, type);
    return virq;
}

struct irq_domain *__irq_domain_add(struct device_node *of_node, int size,
                                    irq_hw_number_t hwirq_max, int direct_max,
                                    struct irq_domain_ops *ops,
                                    void *host_data)
{
    struct irq_domain *domain;

    domain = kzalloc(sizeof (struct irq_domain) +  sizeof(unsigned int) * size, GFP_KERNEL);
    if (WARN_ON(!domain))
        return NULL;

    memset(domain, 0, sizeof(*domain) + sizeof(unsigned int) * size);

    /* Fill structure */
    //INIT_RADIX_TREE(&domain->revmap_tree, GFP_KERNEL);
    domain->ops = ops;
    domain->host_data = host_data;
    domain->of_node = of_node_get(of_node);
    domain->hwirq_max = hwirq_max;
    domain->revmap_size = size;
    domain->revmap_direct_max_irq = direct_max;
    domain->name = of_node->name;
    //irq_domain_check_hierarchy(domain);

    //mutex_lock(&irq_domain_mutex);
    list_add(&domain->link, &irq_domain_list);
    //mutex_unlock(&irq_domain_mutex);

    printk("Added domain %s\n", domain->name);
    return domain;
}

int irq_domain_xlate_onetwocell(struct irq_domain *d,
				struct device_node *ctrlr,
				uint32_t *intspec, unsigned int intsize,
				unsigned long *out_hwirq, unsigned int *out_type)
{
	if (WARN_ON(intsize < 1))
		return -EINVAL;
	*out_hwirq = intspec[0];
	if (intsize > 1)
		*out_type = intspec[1] & IRQ_TYPE_SENSE_MASK;
	else
		*out_type = IRQ_TYPE_NONE;
	return 0;
}

struct irq_domain_ops irq_domain_simple_ops = {
    .xlate = irq_domain_xlate_onetwocell,
};


extern struct irq_chip no_irq_chip;
int irq_domain_set_hwirq_and_chip(struct irq_domain *domain, unsigned int virq,
                                  irq_hw_number_t hwirq, struct irq_chip *chip,
                                  void *chip_data)
{
    struct irq_data *irq_data = irq_domain_get_irq_data(domain, virq);

    if (!irq_data)
        return -ENOENT;

    irq_data->hwirq = hwirq;
    irq_data->chip = chip ? chip : &no_irq_chip;
    irq_data->chip_data = chip_data;

    return 0;
}

void irq_domain_set_info(struct irq_domain *domain, unsigned int virq,
                                irq_hw_number_t hwirq, struct irq_chip *chip,
                                void *chip_data, irq_flow_handler_t handler,
                                void *handler_data, char *handler_name)
{
    irq_domain_set_hwirq_and_chip(domain, virq, hwirq, chip, chip_data);
    __irq_set_handler(virq, handler, 0, handler_name);
    irq_set_handler_data(virq, handler_data);
}